#!/usr/bin/python3
# coding:utf-8

import time
import rospy
from std_msgs.msg import Float32
from control.msg import detect_res
from std_msgs.msg import Float32MultiArray,UInt8
def control_cb_1(data):
    #rospy.loginfo('pos: %s',data.pos)
    #rospy.loginfo('shape: %d',data.shape)
    if abs(data.pos[0]) < 1e-2:
        belt_control_1_pub.publish(0.0)
        array = [2.08, 2.3, -1.85, 0]
        arm_pos = Float32MultiArray(data=array)
        arm_position_1_pub.publish(arm_pos)
        arm_gripple_1_pub.publish(1)
        time.sleep(13)
        array = [2.08, 1.7, -1.85, 0]       
        arm_pos = Float32MultiArray(data=array)
        arm_position_1_pub.publish(arm_pos)
        time.sleep(6)
        arm_gripple_1_pub.publish(0)
        time.sleep(3)
        array = [2.08, 3.0, -1.85, 0]
        arm_pos = Float32MultiArray(data=array)
        arm_position_1_pub.publish(arm_pos)
        if data.shape == 1:
            array = [0.2, 4.0, 0.5, 0]
            arm_pos = Float32MultiArray(data=array)
            arm_position_1_pub.publish(arm_pos)
            array = [-0.3, 4.0, 0.0, 0]
            arm_pos = Float32MultiArray(data=array)
            arm_position_1_pub.publish(arm_pos)
            array = [-0.9, 3.5, -1.0, 0]
            arm_pos = Float32MultiArray(data=array)
            arm_position_1_pub.publish(arm_pos)
            array = [-1.8, 1.8, -1.5, 0]
            arm_pos = Float32MultiArray(data=array)
            arm_position_1_pub.publish(arm_pos)
            time.sleep(25)
            arm_gripple_1_pub.publish(1)
            time.sleep(3)
            array = [2.1, 2.3, -1.0, 0]
            arm_pos = Float32MultiArray(data=array)
            arm_position_1_pub.publish(arm_pos)
            time.sleep(10)
            belt_control_1_pub.publish(0.05)
        elif data.shape == 2:
            array = [0.2, 4.0, 0.5, 0]
            arm_pos = Float32MultiArray(data=array)
            arm_position_1_pub.publish(arm_pos)
            array = [-0.3, 4.0, 0.0, 0]
            arm_pos = Float32MultiArray(data=array)
            arm_position_1_pub.publish(arm_pos)
            array = [-1.8, 1.2, 0.5, 0]
            arm_pos = Float32MultiArray(data=array)
            arm_position_1_pub.publish(arm_pos)
            time.sleep(20)
            arm_gripple_1_pub.publish(1)
            time.sleep(2)
            array = [2.1, 2.3, -1.0, 0]
            arm_pos = Float32MultiArray(data=array)
            arm_position_1_pub.publish(arm_pos)
            time.sleep(10)
            belt_control_1_pub.publish(0.05)
def control_cb_2(data):
    if abs(data.pos[0]) < 5*1e-3:
        belt_control_2_pub.publish(0.0)
        array = [1.8, 3.0, -1.85, 0]
        arm_pos = Float32MultiArray(data=array)
        arm_position_2_pub.publish(arm_pos)
        arm_gripple_2_pub.publish(1)
        time.sleep(15)
        array = [1.8, 1.5, -1.85, 0]       
        arm_pos = Float32MultiArray(data=array)
        arm_position_2_pub.publish(arm_pos)
        time.sleep(5)
        arm_gripple_2_pub.publish(0)
        time.sleep(5)
        array = [1.8, 3.0, -1.85, 0]
        arm_pos = Float32MultiArray(data=array)
        arm_position_2_pub.publish(arm_pos)
def control_cb_3(data):
    if abs(data.pos[0]) < 5*1e-3:
        belt_control_3_pub.publish(0.0)
        array = [2.3, 2.5, -2.0, 0]
        arm_pos = Float32MultiArray(data=array)
        arm_position_3_pub.publish(arm_pos)
        arm_gripple_3_pub.publish(1)
        time.sleep(10)
        array = [2.3, 1.65, -2.0 , 0]       
        arm_pos = Float32MultiArray(data=array)
        arm_position_3_pub.publish(arm_pos)
        time.sleep(5)
        arm_gripple_3_pub.publish(0)
        time.sleep(3)
        arm_gripple_3_pub.publish(0)
        array = [2.3, 1.8, -2.0, 0]
        arm_pos = Float32MultiArray(data=array)
        arm_position_3_pub.publish(arm_pos)
        array = [2.3, 1.8, 1.5, 0]
        arm_pos = Float32MultiArray(data=array)
        arm_position_3_pub.publish(arm_pos)
        array = [2.3, 1.8, 1.5, 1]
        arm_pos = Float32MultiArray(data=array)
        arm_position_3_pub.publish(arm_pos)
        time.sleep(100)
if __name__ == '__main__':
    rospy.init_node('control', anonymous = True)
    rospy.loginfo("Control node is ready.")
    rospy.Subscriber("/detect_res_1",detect_res,control_cb_1,queue_size=1,buff_size=52428800)#处理实时数据
    rospy.Subscriber("/detect_res_2",detect_res,control_cb_2,queue_size=1,buff_size=52428800)
    rospy.Subscriber("/detect_res_3",detect_res,control_cb_3,queue_size=1,buff_size=52428800)
    belt_control_1_pub = rospy.Publisher("/conveyor_belt_1/set_motor_belt_vel",Float32,queue_size=1)
    belt_control_2_pub = rospy.Publisher("/conveyor_belt_2/set_motor_belt_vel",Float32,queue_size=1)
    belt_control_3_pub = rospy.Publisher("/conveyor_belt_3/set_motor_belt_vel",Float32,queue_size=1)  
    arm_gripple_1_pub = rospy.Publisher("/sixdof_1/set_gripple",UInt8,queue_size=1)
    arm_gripple_2_pub = rospy.Publisher("/sixdof_2/set_gripple",UInt8,queue_size=1)
    arm_gripple_3_pub = rospy.Publisher("/sixdof_3/set_gripple",UInt8,queue_size=1)
    arm_position_1_pub = rospy.Publisher("/sixdof_1/set_arm_position",Float32MultiArray,queue_size=50)
    arm_position_2_pub = rospy.Publisher("/sixdof_2/set_arm_position",Float32MultiArray,queue_size=50)
    arm_position_3_pub = rospy.Publisher("/sixdof_3/set_arm_position",Float32MultiArray,queue_size=50)
    rospy.spin()
